module nade.physics;

import nade.vector;
import nade.angle;
import nade.matrix;
import nade.space;
import nade.location;
import nade.collision;


class PhysicsObject : Locatable {
	this(real mass, real inertia)
	{
		_mass = mass;
		_inertia = inertia;
	}

	override void attached()
	{
		assert(location !is null);
		foreach(object; _collisionObjects) location.attach(object);
	}

	void iterate(double delta, double delta2)
	{
		assert(location !is null);

		// _acceleration -= _velocity*0.1;
		// _angularAcceleration -= _angularVelocity*0.1;

		location.move(_velocity*delta + _acceleration*delta2, _angularVelocity*delta + _angularAcceleration*delta2);

		_velocity += _acceleration*delta;
		_angularVelocity += _angularAcceleration*delta;
		_acceleration = Vector2(0, -2.8);
		_angularAcceleration = Radians.ZERO;
	}

	void applyForce(const Vector2 force, const Vector2 point)
	{
		assert(location !is null);
		_acceleration += force/_mass;
		_angularAcceleration += -force.cross(point - position)/_inertia;
	}

	void collision(const Vector2 point, const Vector2 penetration, const Vector2 impulse)
	{
		location.position = position + penetration;

		_velocity += impulse/_mass;
		_angularVelocity += (point - position).cross(impulse)/_inertia;
	}

	PhysicsObject attach(CollisionObject object)
	{
		object.physicsObject = this;
		if(location !is null) location.attach(object);
		_collisionObjects ~= object;

		return this;
	}

	Vector2 momentumAt(const Vector2 point)
	{
		return _velocity*_mass + (position - point).right*(_angularVelocity.radians*_inertia);
	}
	Vector2 velocityAt(const Vector2 point)
	{
		return _velocity - (point - position).cross(_angularVelocity.radians);
	}
	Vector2 accelerationAt(const Vector2 point)
	{
		return _acceleration - (point - position).cross(_angularAcceleration.radians);
	}

	real mass() { return _mass; }
	real inertia() { return _inertia; }

	Vector2 position() { return location.position; }
	Vector2 velocity() { return _velocity; }
	Vector2 acceleration() { return _acceleration; }

	Radians angle() { return location.angle; }
	Radians angularVelocity() { return _angularVelocity; }
	Radians angularAcceleration() { return _angularAcceleration; }

	private {
		real		_mass;
		real		_inertia;

		Vector2		_velocity;
		Radians		_angularVelocity;
		Vector2		_acceleration;
		Radians		_angularAcceleration;

		CollisionObject[]	_collisionObjects;
	}
}


class PhysicsSpace : Space!(PhysicsObject) {
	void iterate(double delta)
	{
		const delta2 = 0.5*delta*delta;
		foreach(object; _objects) object.iterate(delta, delta2);
	}

	PhysicsObject spawn(real mass, real intertia)
	{
		return super.attach(new PhysicsObject(mass, intertia));
	}
}
